From 4e84021573c6ee49f4d11a5c555ac2742157dcb1 Mon Sep 17 00:00:00 2001 From: robertl Date: Sun, 13 Aug 2006 00:16:19 +0000 Subject: [PATCH] Add realtime-tracking infrastructure and readers for Garmin PVT and NMEA. --- compegps.c | 3 ++ defs.h | 23 ++++++++++- garmin.c | 80 ++++++++++++++++++++++++++++++++++++- main.c | 42 ++++++++++++++++++- nmea.c | 115 +++++++++++++++++++++++++++++++++++++++++++++++++++-- ozi.c | 6 +++ pathaway.c | 6 +++ shape.c | 5 ++- stmsdf.c | 3 ++ stmwpp.c | 3 ++ units.c | 8 ++-- 11 files changed, 280 insertions(+), 14 deletions(-) diff --git a/compegps.c b/compegps.c index 1e41df8cc..92f100d75 100644 --- a/compegps.c +++ b/compegps.c @@ -618,6 +618,9 @@ compegps_data_write(void) case rtedata: write_route(); break; + case posndata: + fatal(MYNAME ": Realtime positioning not supported.\n"); + break; } } diff --git a/defs.h b/defs.h index 5a3400054..736e21d14 100644 --- a/defs.h +++ b/defs.h @@ -124,19 +124,22 @@ typedef enum { typedef enum { trkdata = 1 , wptdata, - rtedata + rtedata, + posndata } gpsdata_type; #define NOTHINGMASK 0 #define WPTDATAMASK 1 #define TRKDATAMASK 2 #define RTEDATAMASK 4 +#define POSNDATAMASK 8 /* mask objective testing */ #define doing_nothing (global_opts.masked_objective == NOTHINGMASK) #define doing_wpts ((global_opts.masked_objective & WPTDATAMASK) == WPTDATAMASK) #define doing_trks ((global_opts.masked_objective & TRKDATAMASK) == TRKDATAMASK) #define doing_rtes ((global_opts.masked_objective & RTEDATAMASK) == RTEDATAMASK) +#define doing_posn ((global_opts.masked_objective & POSNDATAMASK) == POSNDATAMASK) typedef struct { int synthesize_shortnames; @@ -390,6 +393,8 @@ typedef void (*ff_deinit) (void); typedef void (*ff_read) (void); typedef void (*ff_write) (void); typedef void (*ff_exit) (void); +typedef void (*ff_writeposn) (waypoint *); +typedef waypoint * (*ff_readposn) (void); #ifndef DEBUG_MEM char * get_option(const char *iarglist, const char *argname); @@ -572,6 +577,19 @@ typedef enum { #define FF_CAP_RW_WPT \ { ff_cap_read | ff_cap_write, ff_cap_none, ff_cap_none} +/* + * Format capabilities for realtime positioning. + */ +typedef struct position_ops { + ff_init rd_init; + ff_readposn rd_position; + ff_deinit rd_deinit; + + ff_init wr_init; + ff_writeposn wr_position; + ff_deinit wr_deinit; +} position_ops_t; + /* * Describe the file format to the caller. */ @@ -588,6 +606,7 @@ typedef struct ff_vecs { arglist_t *args; char *encode; int fixed_encode; + position_ops_t position_ops; } ff_vecs_t; typedef struct style_vecs { @@ -772,7 +791,7 @@ unsigned long get_crc32_s(const void * data); */ typedef enum { units_unknown = 0, - units_statue = 1, + units_statute = 1, units_metric =2 } fmt_units; diff --git a/garmin.c b/garmin.c index 219ea7f2a..054c9c28e 100644 --- a/garmin.c +++ b/garmin.c @@ -409,6 +409,81 @@ route_read(void) } +/* + * Rather than propogate Garmin-specific data types outside of the Garmin + * code, we convert the PVT (position/velocity/time) data from the receiver + * to the data type we use throughout. Yes, we do lose some data that way. + */ +static void +pvt2wpt(GPS_PPvt_Data pvt, waypoint *wpt) +{ + double wptime, wptimes; + + wpt->altitude = pvt->alt; + wpt->latitude = pvt->lat; + wpt->longitude = pvt->lon; + + /* + * The unit reports time in three fields: + * 1) The # of days to most recent Sun. since 1989-12-31 midnight UTC. + * 2) The number of seconds (fractions allowed) since that Sunday. + * 3) The number of leap seconds that offset the current UTC and GPS + * reference clocks. + */ + wptime = 631065600.0 + pvt->wn_days * 86400.0 + + pvt->tow + - pvt->leap_scnds; + wptimes = floor(wptime); + wpt->creation_time = wptimes; + wpt->centiseconds = 100.0 * (wptime - wptimes); + + /* + * The Garmin spec fifteen different models that use a different + * table for 'fix' without a really good way to tell if the model + * we're talking to happens to be one of those...By inspection, + * it looks like even though the models (Summit, Legend, etc.) may + * be popular, it's older (2001 and earlier or so) versions that + * are affected and I think there are relatively few readers of + * the fix field anyway. Time will tell if this is a good plan. + */ + switch (pvt->fix) { + case 0: wpt->fix = fix_unknown;break; + case 1: wpt->fix = fix_none;break; + case 2: wpt->fix = fix_2d;break; + case 3: wpt->fix = fix_3d;break; + case 4: wpt->fix = fix_dgps;break; /* 2D_diff */ + case 5: wpt->fix = fix_dgps;break; /* 3D_diff */ + default: + /* undocumented type. */ + break; + } +} + +static gpsdevh *pvt_fd; + +static void +pvt_init(const char *fname) +{ + rw_init(fname); + GPS_Command_Pvt_On(fname, &pvt_fd); +} + +static waypoint * +pvt_read(void) +{ + waypoint *wpt = waypt_new(); + GPS_PPvt_Data pvt = GPS_Pvt_New(); + + if (GPS_Command_Pvt_Get(&pvt_fd, &pvt)) { + pvt2wpt(pvt, wpt); + wpt->shortname = xstrdup("Position"); + + return wpt; + } + + return NULL; +} + static void data_read(void) { @@ -423,7 +498,7 @@ data_read(void) if (global_opts.masked_objective & RTEDATAMASK) route_read(); if (!(global_opts.masked_objective & - (WPTDATAMASK | TRKDATAMASK | RTEDATAMASK))) + (WPTDATAMASK | TRKDATAMASK | RTEDATAMASK | POSNDATAMASK))) fatal(MYNAME ": Nothing to do.\n"); } @@ -739,7 +814,8 @@ ff_vecs_t garmin_vecs = { data_write, NULL, garmin_args, - CET_CHARSET_ASCII, 0 + CET_CHARSET_ASCII, 0, + { pvt_init, pvt_read, NULL, NULL, NULL, NULL } }; static const char *d103_icons[16] = { diff --git a/main.c b/main.c index 8296fde99..6acee034e 100644 --- a/main.c +++ b/main.c @@ -203,6 +203,10 @@ main(int argc, char *argv[]) if (ivecs->rd_init == NULL) { fatal ("Format does not support reading.\n"); } + if (global_opts.masked_objective & POSNDATAMASK) { + did_something = 1; + break; + } /* simulates the default behaviour of waypoints */ if (doing_nothing) global_opts.masked_objective |= WPTDATAMASK; @@ -221,7 +225,7 @@ main(int argc, char *argv[]) optarg = argv[argn][2] ? argv[argn]+2 : argv[++argn]; ofname = optarg; - if (ovecs) { + if (ovecs && (!(global_opts.masked_objective & POSNDATAMASK))) { /* simulates the default behaviour of waypoints */ if (doing_nothing) global_opts.masked_objective |= WPTDATAMASK; @@ -287,6 +291,10 @@ main(int argc, char *argv[]) global_opts.objective = rtedata; global_opts.masked_objective |= RTEDATAMASK; break; + case 'T': + global_opts.objective = posndata; + global_opts.masked_objective |= POSNDATAMASK; + break; case 'N': switch(argv[argn][2]) { case 'i': @@ -423,6 +431,38 @@ main(int argc, char *argv[]) global_opts.verbose_status = saved_status; } + /* + * This is very unlike the rest of our command sequence. + * If we're doing realtime position tracking, we enforce that + * we're not doing anything else and we just bounce between + * the special "read position" and "write position" vectors + * in our most recent vecs. + */ + if (global_opts.masked_objective & POSNDATAMASK) { + waypoint *wpt = waypt_new(); + ivecs->position_ops.rd_init(fname); + + if (global_opts.masked_objective & ~POSNDATAMASK) { + fatal("Realtime tracking (-T) is exclusive of other modes.\n"); + } + + while (1) { + wpt = ivecs->position_ops.rd_position(); + if (wpt) { + if (ovecs) { + ovecs->position_ops.wr_init(ofname); + ovecs->position_ops.wr_position(wpt); + ovecs->position_ops.wr_deinit(); + } else { + /* Just print to screen */ + waypt_disp(wpt); + } + } + } +// waypt_del(wpt); + } + + if (!did_something) fatal ("Nothing to do! Use '%s -h' for command-line options.\n", prog_name); diff --git a/nmea.c b/nmea.c index cc4e30469..d29f4a908 100644 --- a/nmea.c +++ b/nmea.c @@ -24,6 +24,7 @@ #include #include "defs.h" +#include "gbser.h" #include "strptime.h" /********************************************************** @@ -139,6 +140,12 @@ typedef enum { gprmc } preferred_posn_type; +enum { + rm_unknown = 0, + rm_serial, + rm_file +} read_mode; + static FILE *file_in; static FILE *file_out; static route_head *trk_head; @@ -147,6 +154,7 @@ static preferred_posn_type posn_type; static struct tm tm; static waypoint * curr_waypt = NULL; static waypoint * last_waypt = NULL; +static void * gbser_handle; static int without_date; /* number of created trackpoints without a valid date */ static struct tm opt_tm; /* converted "date" parameter */ @@ -162,12 +170,18 @@ static char *dogpvtg = NULL; static char *dogpgsa = NULL; static char *snlenopt = NULL; static char *optdate = NULL; +static char *getposnarg = NULL; static char *opt_sleep = NULL; +static char *opt_baud = NULL; static long sleepus = 0; +static int getposn; static time_t last_time = -1; static double last_read_time; /* Last timestamp of GGA or PRMC */ +static waypoint * nmea_rd_posn(void); +static void nmea_rd_posn_init(const char *fname); + arglist_t nmea_args[] = { {"snlen", &snlenopt, "Max length of waypoint name to write", "6", ARGTYPE_INT, "1", "64" }, {"gprmc", &dogprmc, "Read/write GPRMC sentences", "1", ARGTYPE_BOOL, ARG_NOMINMAX }, @@ -175,8 +189,10 @@ arglist_t nmea_args[] = { {"gpvtg", &dogpvtg, "Read/write GPVTG sentences", "1", ARGTYPE_BOOL, ARG_NOMINMAX }, {"gpgsa", &dogpgsa, "Read/write GPGSA sentences", "1", ARGTYPE_BOOL, ARG_NOMINMAX }, {"date", &optdate, "Complete date-free tracks with given date (YYYYMMDD).", NULL, ARGTYPE_INT, ARG_NOMINMAX }, - {"pause", &opt_sleep, "Decimal seconds to pause between groups of strings", NULL, - ARGTYPE_STRING, ARG_NOMINMAX }, + { "get_posn", &getposnarg, "Return current position as a waypoint", + NULL, ARGTYPE_BOOL, ARG_NOMINMAX}, + {"pause", &opt_sleep, "Decimal seconds to pause between groups of strings", NULL, ARGTYPE_INT, ARG_NOMINMAX }, + {"baud", &opt_baud, "Bits per speed of serial port (baud=4800)", NULL, ARGTYPE_INT, ARG_NOMINMAX }, ARG_TERMINATOR }; @@ -200,13 +216,48 @@ nmea_rd_init(const char *fname) { curr_waypt = NULL; last_waypt = NULL; + + if (getposnarg) { + getposn = 1; + } + + /* A special case hack that gets our current position and returns + * it as one waypoint. + */ + if (getposn) { + waypoint *wpt; + nmea_rd_posn_init(fname); + wpt = nmea_rd_posn(); + if (!wpt) { + return; + } + if (wpt->shortname) { + xfree(wpt->shortname); + } + wpt->shortname = xstrdup("Position"); + waypt_add(wpt); + return; + } + + read_mode = rm_file; file_in = xfopen(fname, "rb", MYNAME); } static void nmea_rd_deinit(void) { - fclose(file_in); + switch(read_mode) { + case rm_serial: + gbser_deinit(gbser_handle); + break; + case rm_file: + fclose(file_in); + file_in = NULL; + break; + default: + fatal("nmea_rd_deinit: illegal read_mode.\n"); + break; + } } static void @@ -718,6 +769,11 @@ nmea_read(void) without_date = 0; memset(&tm, 0, sizeof(tm)); opt_tm = tm; + + /* This was done in rd_init() */ + if (getposn) { + return; + } if (optdate) { @@ -750,6 +806,56 @@ nmea_read(void) textfile_done(tin); } +void +nmea_rd_posn_init(const char *fname) +{ + if ((gbser_handle = gbser_init(fname)) != NULL) { + read_mode = rm_serial; + gbser_set_speed(gbser_handle, 4800); + } else { + fatal("Could not open %s\n", fname); + } + + if (opt_baud) { + if (!gbser_set_speed(gbser_handle, atoi(opt_baud))) { + fatal("Unable to set baud rate %s\n", opt_baud); + } + } +} + +static waypoint * +nmea_rd_posn(void) +{ + char ibuf[1024]; + static double lt = -1; + int i; + + /* + * Read a handful of sentences, collecting the best info we + * can. If the timestamp changes (indicating the sequence is + * about to restart and thus the one we're collecting isn't going + * to get any better than we now have) hand that back to the caller. + */ + for (i = 0; i < 10; i++) { + int rv; + ibuf[0] = 0; + rv = gbser_read_line(gbser_handle, ibuf, sizeof(ibuf), 2000, '\x0a', '\x0d'); + if (global_opts.debug_level > 1) { + warning( "READ: %s\n", ibuf); + } + if (rv < 0) { + fatal("No data received.\n"); + } + nmea_parse_one_line(ibuf); + if (lt != last_read_time) { + if (last_read_time) { + lt = last_read_time; + return waypt_dupe(curr_waypt); + } + } + } + return NULL; +} static void nmea_wayptpr(const waypoint *wpt) @@ -921,5 +1027,6 @@ ff_vecs_t nmea_vecs = { nmea_write, NULL, nmea_args, - CET_CHARSET_ASCII, 0 /* CET-REVIEW */ + CET_CHARSET_ASCII, 0, /* CET-REVIEW */ + { nmea_rd_posn_init, nmea_rd_posn, nmea_rd_deinit, NULL, NULL, NULL } }; diff --git a/ozi.c b/ozi.c index e90c50927..6850eef00 100644 --- a/ozi.c +++ b/ozi.c @@ -637,6 +637,9 @@ data_read(void) case wptdata: ozi_parse_waypt(i, s, wpt_tmp, fsdata); break; + case posndata: + fatal(MYNAME ": realtime positioning not supported.\n"); + break; } i++; s = csv_lineparse(NULL, ",", "", linecount); @@ -664,6 +667,9 @@ data_read(void) waypt_free(wpt_tmp); } break; + case posndata: + fatal(MYNAME ": realtime positioning not supported.\n"); + break; } } else { diff --git a/pathaway.c b/pathaway.c index d68193715..5aa57ed52 100644 --- a/pathaway.c +++ b/pathaway.c @@ -526,6 +526,9 @@ static void ppdb_read(void) case wptdata: ppdb_read_wpt(pdb_in, pdb_rec, NULL, 0); break; + case posndata: + fatal(MYNAME ": Realtime positioning not supported.\n"); + break; } free_pdb(pdb_in); @@ -716,6 +719,9 @@ static void ppdb_write(void) appinfo->dataBaseSubType = 1; route_disp_all(ppdb_track_header, ppdb_track_trailer, ppdb_write_wpt); break; + case posndata: + fatal(MYNAME ": Realtime positioning not supported.\n"); + break; } pdb_Write(pdb_out, fileno(fd_out)); diff --git a/shape.c b/shape.c index fdfe2cf02..109541238 100644 --- a/shape.c +++ b/shape.c @@ -295,7 +295,10 @@ my_write(void) route_disp_all(poly_init, poly_deinit, poly_point); break; case rtedata: - fatal(MYNAME ":Routes are not supported\n"); + fatal(MYNAME ": Routes are not supported\n"); + break; + case posndata: + fatal(MYNAME ": Realtime positioning not supported\n"); break; } } diff --git a/stmsdf.c b/stmsdf.c index a793e9f2c..b90ce8bdd 100644 --- a/stmsdf.c +++ b/stmsdf.c @@ -692,6 +692,9 @@ data_write(void) } } break; + case posndata: + fatal(MYNAME ": Realtime positioning not supported.\n"); + break; } } diff --git a/stmwpp.c b/stmwpp.c index 4a765f0a7..0b0ee5fc1 100644 --- a/stmwpp.c +++ b/stmwpp.c @@ -287,6 +287,9 @@ stmwpp_data_write(void) what = STM_TRKPT; track_disp_all(stmwpp_track_hdr, stmwpp_track_tlr, stmwpp_waypt_cb); break; + case posndata: + fatal(MYNAME ": Realtime positioning not supported.\n"); + break; } } diff --git a/units.c b/units.c index 0b8f93b72..d3a85bfed 100644 --- a/units.c +++ b/units.c @@ -21,13 +21,13 @@ #include "defs.h" -static int units = units_statue; +static int units = units_statute; int fmt_setunits(fmt_units u) { switch (u) { - case units_statue: + case units_statute: case units_metric: units = u; return 0; @@ -42,7 +42,7 @@ fmt_distance(const double distance_meters, char **tag) double d; switch (units) { - case units_statue: + case units_statute: d = METERS_TO_FEET(distance_meters); if (d < 5280) { *tag = "ft"; @@ -75,7 +75,7 @@ fmt_speed(const double distance_meters_sec, char **tag) double d; switch (units) { - case units_statue: + case units_statute: d = METERS_TO_MILES(distance_meters_sec) * SECONDS_PER_HOUR ; *tag = "mph"; break; -- 2.30.2